/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.neu.nutrons.bots.logomotion;

import edu.neu.nutrons.bots.logomotion.sim.*;
import edu.neu.nutrons.lib.MotorMechanism;
import edu.neu.nutrons.lib.NutronsQueue;
import edu.wpi.first.wpilibj.Solenoid;

/**
 *
 * @author NUTRONS_PROGRAMMING
 */
public class Elevator {
    // preset positions
    public static final ElevatorCommand EC_HOME = new ElevatorCommand("Home",0, false, false);
    public static final ElevatorCommand EC_LOW = new ElevatorScoringCommand("Low",.2, false, false);
    public static final ElevatorCommand EC_MID = new ElevatorScoringCommand("Medium",.6, false, false);
    public static final ElevatorCommand EC_HIGH = new ElevatorScoringCommand("High",1, false, false);

    MotorMechanism liftDrive = new LiftDriveSim();
    ElevatorCommand curCmd = null;
    Solenoid wrist = new Solenoid(1); // change #
    Solenoid claw = new Solenoid(2); // change #
    NutronsQueue cmdList = new NutronsQueue();
    boolean offset = false;
    boolean getNewCommand = true;

    int loops = 0;

    public Elevator(){
        cmdList.push(EC_HOME);
    }

    public void handle(){
        double speedCmd = 0;
        
        if(getNewCommand) { // no queued command
            ElevatorCommand nextCmd = (ElevatorCommand) cmdList.pop();
            if(nextCmd != null){
                curCmd = nextCmd;
            }
            getNewCommand = false;
        }

        // safety thing
        if(curCmd == null)
        {
            getNewCommand = true;
            if(cmdList.isEmpty()){
                liftDrive.set(0);
                return;
            }
        }

        if(curCmd != null) {
            speedCmd = curCmd.calcSpeed(liftDrive.getPos());
            wrist.set(curCmd.getWrist());
            claw.set(curCmd.getClaw());
            // Do we need  a new command?

            if(curCmd.isDone()){
                if(cmdList.isEmpty()){
                    ElevatorCommand idle = new ElevatorCommand("IDLE",curCmd.getPos(), curCmd.getWrist(), curCmd.getClaw());
                    setCommand(idle);
                }
                else {
                    getNewCommand = true;
                }
                
            }
        }
        liftDrive.set(speedCmd);


        // debug info remove later
        if(loops % 40 == 0){
            //System.out.println("in hereh...");
            System.out.println("CMD: " + curCmd.name + " SP: " + curCmd.pos + "Sens: " + curCmd.lastFB);
        }
        loops++;
    }

    public void addCommand(ElevatorCommand cmd) {
        cmdList.push(cmd);
    }

    public void setCommand(ElevatorCommand cmd) {
        cmdList.removeAllElements();
        curCmd = cmd;
    }

    public void doOffset(){
        if(curCmd != null) {
            curCmd.doOffset();
        }
    }
}
